﻿#include "cybertron/gis/ECEF.hpp"
#include <cmath>

CYBERTRON_BEGIN

static double a = 6378137.0; //WGS-84 semi-major axis
static double e2 = 6.6943799901377997e-3; //WGS-84 first eccentricity squared
static double a1 = 4.2697672707157535e+4; //a1 = a*e2
static double a2 = 1.8230912546075455e+9; //a2 = a1*a1
static double a3 = 1.4291722289812413e+2; //a3 = a1*e2/2
static double a4 = 4.5577281365188637e+9; //a4 = 2.5*a2
static double a5 = 4.2840589930055659e+4; //a5 = a1+a3
static double a6 = 9.9330562000986220e-1; //a6 = 1-e2

/*
struct Vector3d
{
	double x, y, z;

	Vector3d() {
		x = y = z = 0;
	}
	Vector3d(double xx, double yy, double zz)
	{
		x = xx;
		y = yy;
		z = zz;
	}

	Vector3d operator + (const Vector3d& rhs) const
	{
		return Vector3d(x + rhs.x, y + rhs.y, z + rhs.z);
	}

	Vector3d operator - (const Vector3d& rhs) const
	{
		return Vector3d(x - rhs.x, y - rhs.y, z - rhs.z);
	}

	Vector3d operator * (double f) const
	{
		return Vector3d(x * f, y * f, z * f);
	}
};

inline Vector3d operator * (double lhs, const Vector3d& rhs)
{
	return Vector3d(lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
}

inline double dot(const Vector3d& lhs, const Vector3d& rhs)
{
	return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z;
}

static Vector3d sCenter(-2897386.44850561, 4649992.43536783, 3254530.5603273);
static Vector3d sAxisX(-0.84872421560653, -0.528835707798821, 2.76448987574751E-15);
static Vector3d sAxisY(0.270082631903257, -0.433453464905283, 0.859751979181098);
static Vector3d sAxisZ(-0.454667546441673, 0.729692324146639, 0.510711791810396);

// 输入值：地图坐标系中的本地坐标 (X, Y, 高度)
// 返回值：WGS84 经纬度坐标 (纬度，经度，高度)，均以弧度表示
Vector3d ConvertLocalPointToWGS84(const Vector3d& localPoint)
{
	Vector3d ecef = sCenter + localPoint.x * sAxisX + localPoint.y * sAxisY + localPoint.z * sAxisZ;

	double zp, w2, w, r2, r, s2, c2, s, c, ss;
	double g, rg, rf, u, v, m, f, p, x, y, z;

	Vector3d geo; //Results go here (Lat, Lon, Altitude)
	x = ecef.x;
	y = ecef.y;
	z = ecef.z;
	zp = std::abs(z);
	w2 = x * x + y * y;
	w = std::sqrt(w2);
	r2 = w2 + z * z;
	r = std::sqrt(r2);
	geo.y = std::atan2(y, x); //Lon (final)
	s2 = z * z / r2;
	c2 = w2 / r2;
	u = a2 / r;
	v = a3 - a4 / r;
	if (c2 > 0.3)
	{
		s = (zp / r) * (1.0 + c2 * (a1 + u + s2 * v) / r);
		geo.x = std::asin(s); //Lat
		ss = s * s;
		c = std::sqrt(1.0 - ss);
	}
	else
	{
		c = (w / r) * (1.0 - s2 * (a5 - u - c2 * v) / r);
		geo.x = std::acos(c); //Lat
		ss = 1.0 - c * c;
		s = std::sqrt(ss);
	}
	g = 1.0 - e2 * ss;
	rg = a / std::sqrt(g);
	rf = a6 * rg;
	u = w - rg * c;
	v = zp - rf * s;
	f = c * u + s * v;
	m = c * v - s * u;
	p = m / (rf / g + f);
	geo.x = geo.x + p; //Lat
	geo.z = f + m * p / 2.0; //Altitude
	if (z < 0.0)
	{
		geo.x *= -1.0; //Lat
	}
	return (geo); //Return Lat, Lon, Altitude in that order
}

// 输入值：WGS84 经纬度坐标 (纬度，经度，高度)，均以弧度表示
// 返回值：地图坐标系中的本地坐标 (X, Y, 高度)
Vector3d ConvertWGS84ToLocalPoint(const Vector3d& geo)
{
	double n, lat, lon, alt;

	Vector3d ecef;
	lat = geo.x;
	lon = geo.y;
	alt = geo.z;
	n = a / std::sqrt(1 - e2 * std::sin(lat) * std::sin(lat));
	ecef.x = (n + alt) * std::cos(lat) * std::cos(lon); //ECEF x
	ecef.y = (n + alt) * std::cos(lat) * std::sin(lon); //ECEF y
	ecef.z = (n * (1 - e2) + alt) * std::sin(lat); //ECEF z

	Vector3d delta = ecef - sCenter;
	return Vector3d(
		dot(delta, sAxisX),
		dot(delta, sAxisY),
		dot(delta, sAxisZ));
}
*/

CybertronCoreAPI dvec3 ECEF::toGeodetic(const dvec3& ecef)
{
	double zp, w2, w, r2, r, s2, c2, s, c, ss;
	double g, rg, rf, u, v, m, f, p, x, y, z;

	dvec3 geo; //Results go here (Lat, Lon, Altitude)
	x = ecef.x;
	y = ecef.y;
	z = ecef.z;
	zp = std::abs(z);
	w2 = x * x + y * y;
	w = std::sqrt(w2);
	r2 = w2 + z * z;
	r = std::sqrt(r2);
	geo.y = std::atan2(y, x); //Lon (final)
	s2 = z * z / r2;
	c2 = w2 / r2;
	u = a2 / r;
	v = a3 - a4 / r;
	if (c2 > 0.3)
	{
		s = (zp / r) * (1.0 + c2 * (a1 + u + s2 * v) / r);
		geo.x = std::asin(s); //Lat
		ss = s * s;
		c = std::sqrt(1.0 - ss);
	}
	else
	{
		c = (w / r) * (1.0 - s2 * (a5 - u - c2 * v) / r);
		geo.x = std::acos(c); //Lat
		ss = 1.0 - c * c;
		s = std::sqrt(ss);
	}
	g = 1.0 - e2 * ss;
	rg = a / std::sqrt(g);
	rf = a6 * rg;
	u = w - rg * c;
	v = zp - rf * s;
	f = c * u + s * v;
	m = c * v - s * u;
	p = m / (rf / g + f);
	geo.x = geo.x + p; //Lat
	geo.z = f + m * p / 2.0; //Altitude
	if (z < 0.0)
	{
		geo.x *= -1.0; //Lat
	}
	return (geo); //Return Lat, Lon, Altitude in that order
}

CybertronCoreAPI dvec3 ECEF::fromGeodetic(const dvec3& geo)
{
	double n, lat, lon, alt;

	dvec3 ecef;
	lat = geo.x;
	lon = geo.y;
	alt = geo.z;
	n = a / std::sqrt(1 - e2 * std::sin(lat) * std::sin(lat));
	ecef.x = (n + alt) * std::cos(lat) * std::cos(lon); //ECEF x
	ecef.y = (n + alt) * std::cos(lat) * std::sin(lon); //ECEF y
	ecef.z = (n * (1 - e2) + alt) * std::sin(lat); //ECEF z
	return (ecef); //Return x, y, z in ECEF
}

CYBERTRON_END
